#!/usr/bin/env python


import rospy
from sender.msg import motor_data
from std_msgs.msg import String

status_dic = {}
changing_dic = {}
cur_status =None

class  status:
    name = 'undefined'
    end = None

    def __init__(self, name):
        self.name = name
        status_dic[name] = self
        self.edges={}

    def connect_to_status(self, con_satus,chg_class):

        if not status_dic.has_key(con_satus):
            rospy.logerr("no status :%s",con_satus)
            return

        self.edges[con_satus]=chg_class.run


    def show(self):
        rospy.loginfo(self.name)
    def change_to(self,name):
        global cur_status
        rospy.loginfo('change %s to %s',self.name,name)
        if self.edges.has_key(name):
            self.edges[name]()
            cur_status=status_dic[name]
        else:
            rospy.logerr("change status :%s", name)
class changing:
    pub =rospy.Publisher('/motion/motor',motor_data,queue_size=10)

    def __init__(self,fun,rate=100):
        self.fun=fun
        self.rate = rospy.Rate(rate) #
        pass
    def run(self):
        rate = rospy.Rate(100) # 10hz
        rospy.set_param('/motion/changing/process',0)
        process=0
        while not rospy.is_shutdown() and process<1 :
            process+=self.fun(self.pub,process)
            rospy.set_param('/motion/changing/process',process)
            rate.sleep()
        rospy.set_param('/motion/changing/process',0)
##functions for changing
def shake_hand(pub,process):
    data = motor_data()
    data.data[2]=process*300
    data.map[2]=True
    data.header.stamp=rospy.get_rostime()
    data.header.frame_id='shake'
    pub.publish(data)
    return 0.01

def shake_hip(pub,process):
    data = motor_data()
    data.data[3]=process*300
    data.map[3]=True
    data.header.stamp=rospy.get_rostime()
    data.header.frame_id='hip'
    pub.publish(data)
    return 0.01

# data: string
def changeState(data ):
    global cur_status
    process = rospy.get_param('/motion/changing/process')
    if process == 0:
        cur_status.change_to(data.data)
    else:
        rospy.loginfo("%s is running",cur_status.name)
if __name__ == '__main__':
    rospy.init_node('state_manager', anonymous=True)
    sub=rospy.Subscriber('/motion/state',String,changeState,queue_size=1)
    s1=status('s1')
    s2=status('s2')
    s1.connect_to_status('s2',changing(shake_hand))
    s1.connect_to_status('s1',changing(shake_hand))
    s2.connect_to_status('s1',changing(shake_hip))
    global cur_status
    cur_status=s1

    rospy.spin()



